module tb_cordic_unit();
  
  reg [15:0] x_in, y_in, teta_in;
  reg start, clk, rst_n;
  
  wire [15:0] x_out, y_out, atan;
  
  wire [3:0] count;
  integer i;
  real angle_degree, cos, sin;
  
  task reset;
    begin
      clk = 0;
      rst_n = 0;
      start = 0;
      #50
      rst_n = 1;
    end
  endtask
  
  //assign atan = arc_tg(count);
  assign atan = arc_tg_norm(count);
    
  cordic_unit CORDIC
  (
    .x_out    ( x_out    ),
    .y_out    ( y_out    ),
    .ready    ( ready    ),
    .x_in     ( x_in     ),
    .y_in     ( y_in     ),
    .teta_in  ( teta_in  ),
    .count    ( count    ),
    .arc_tg   ( atan     ),
    .start    ( start    ),
    .clk      ( clk      ),
    .rst_n    ( rst_n    )
  );

//Format: Q1.15
function [15:0] arc_tg;
  input [3:0] i;
  begin
    case (i)
      4'd0 : arc_tg = 16'd25736;
      4'd1 : arc_tg = 16'd15193;
      4'd2 : arc_tg = 16'd8027;
      4'd3 : arc_tg = 16'd4075;
      4'd4 : arc_tg = 16'd2045;
      4'd5 : arc_tg = 16'd1024;
      4'd6 : arc_tg = 16'd512;
      4'd7 : arc_tg = 16'd256;
      4'd8 : arc_tg = 16'd128;
      4'd9 : arc_tg = 16'd64;
      4'd10: arc_tg = 16'd32;
      4'd11: arc_tg = 16'd16;
      4'd12: arc_tg = 16'd8;
      4'd13: arc_tg = 16'd4;
      4'd14: arc_tg = 16'd2;
      4'd15: arc_tg = 16'd1;
    endcase
  end
endfunction

//Normalized format
function [15:0] arc_tg_norm;
  input [3:0] i;
  begin
    case (i)
      4'd0 : arc_tg_norm = 16'd8192;
      4'd1 : arc_tg_norm = 16'd4836;
      4'd2 : arc_tg_norm = 16'd2555;
      4'd3 : arc_tg_norm = 16'd1297;
      4'd4 : arc_tg_norm = 16'd651;
      4'd5 : arc_tg_norm = 16'd326;
      4'd6 : arc_tg_norm = 16'd163;
      4'd7 : arc_tg_norm = 16'd81;
      4'd8 : arc_tg_norm = 16'd41;
      4'd9 : arc_tg_norm = 16'd20;
      4'd10: arc_tg_norm = 16'd10;
      4'd11: arc_tg_norm = 16'd5;
      4'd12: arc_tg_norm = 16'd3;
      4'd13: arc_tg_norm = 16'd1;
      4'd14: arc_tg_norm = 16'd1;
      4'd15: arc_tg_norm = 16'd0;
    endcase
  end
endfunction

function [21:0] arc_tg_22;
  input [4:0] i;
  begin
    case(i)
      5'd0: arc_tg_22 = 22'd524288;
      5'd1: arc_tg_22 = 22'd309505;
      5'd2: arc_tg_22 = 22'd163534;
      5'd3: arc_tg_22 = 22'd83012;
      5'd4: arc_tg_22 = 22'd41667;
      5'd5: arc_tg_22 = 22'd20854;
      5'd6: arc_tg_22 = 22'd10430;
      5'd7: arc_tg_22 = 22'd5215;
      5'd8: arc_tg_22 = 22'd2608;
      5'd9: arc_tg_22 = 22'd1304;
      5'd10: arc_tg_22 = 22'd652;
      5'd11: arc_tg_22 = 22'd326;
      5'd12: arc_tg_22 = 22'd163;
      5'd13: arc_tg_22 = 22'd81;
      5'd14: arc_tg_22 = 22'd41;
      5'd15: arc_tg_22 = 22'd20;
      5'd16: arc_tg_22 = 22'd10;
      5'd17: arc_tg_22 = 22'd5;
      5'd18: arc_tg_22 = 22'd3;
      5'd19: arc_tg_22 = 22'd1;
      5'd20: arc_tg_22 = 22'd1;
      5'd21: arc_tg_22 = 22'd0;
    endcase
 end
endfunction

task comp_sin_cos;
  real sin, cos;
  begin
    @(negedge clk);
    x_in = 16'd61558;//sd9945;//19896;
    y_in = 16'd59569;//sd9945;
    for(i = 0; i <= 360; i = i + 5)
      begin
        teta_in = $signed(182*i);
        start = 1;
        @(negedge clk);
        start = 0;
        @(posedge ready);
        angle_degree = 0.0055 * teta_in;
        cos = $signed(x_out)/32768.0;
        sin = $signed(y_out)/32768.0;
        $display("Angle: %0d", angle_degree);
        $display("cos = %f",cos);
        $display("sin = %f",sin);
        @(negedge clk);
        @(negedge clk);
     end
  end
endtask

initial
  begin
    reset;
    comp_sin_cos;
        /*
        teta_in = angle(i);
        start = 1;
        @(negedge clk);
        start = 0;
        @(posedge ready);
        angle_degree = 0.0055 * teta_in;
        cos = x_out/32768.0;
        sin = y_out/32768.0;
        */
        
        
    //teta_in = 16'sd8192;//16'sd25736;
    //start = 1;
    //@(negedge clk)
    //start = 0;
  end

always #10
  clk = !clk;
endmodule
